#include <UnitTest++.h>
#include "config.h"
#include "grid.h"

using namespace mcrx;
using namespace std;

typedef adaptive_grid<int> T_grid;
typedef T_grid::T_grid T_grid_impl;
typedef typename T_grid::T_cell T_cell;
typedef typename T_grid::T_cell_tracker T_cell_tracker;
typedef typename T_grid::T_qpoint T_qpoint;
typedef typename T_cell_tracker::T_code T_code;

struct single_cell_fixture {
  single_cell_fixture() : min(0,0,0), max(1,1,1), n(1,1,1), g(min, max) {};

  vec3d min;
  vec3d max;
  vec3i n;

  T_grid g;
};

struct single_subtree_fixture {
  single_subtree_fixture() : 
    min(0,0,0), max(1,1,1), n(1,1,1), g(min, max) {
    c=g.begin();
    c.cell()->refine();
  };

  vec3d min;
  vec3d max;
  vec3i n;

  T_grid g;
  T_cell_tracker c;
};


TEST_FIXTURE(single_cell_fixture, grid_dims)
{
  CHECK_ARRAY_EQUAL(min,g.getmin(),3);
  CHECK_ARRAY_EQUAL(max,g.getmax(),3);

  CHECK(g.in_grid(vec3d(0.5,0.5,0.5)));
  CHECK(!g.in_grid(vec3d(-0.5,0.5,0.5)));
  CHECK(!g.in_grid(vec3d(1.5,0.5,0.5)));
}

TEST_FIXTURE(single_cell_fixture, single_cell)
{
  T_cell_tracker c= g.begin();
  CHECK(c);
  CHECK_ARRAY_EQUAL(g.getmin(), c.getmin(), 3);
  CHECK_ARRAY_EQUAL(g.getmax(), c.getmax(), 3);
  CHECK_ARRAY_EQUAL(g.getsize(), c.getsize(), 3);
  CHECK(c.is_leaf());
  CHECK_ARRAY_EQUAL(vec3d(.5,.5,.5), c.getcenter(), 3);
  CHECK_EQUAL(1, c.volume());
  CHECK_EQUAL(T_qpoint(0,0,0,0), c.qpos());
}

// test that we can traverse a octree just consisting of the root node
TEST_FIXTURE(single_cell_fixture, dfs_traversal1)
{
  T_cell_tracker c= g.begin();
  CHECK_EQUAL(hilbert::qpoint(0,0,0,0), c.qpos());
  c.dfs();
  CHECK(c.at_end());
}

// test that we can traverse a octree just consisting of the root node
TEST_FIXTURE(single_cell_fixture, bidir_dfs_traversal1)
{
  T_cell_tracker c= g.begin();
  CHECK_EQUAL(hilbert::qpoint(0,0,0,0), c.qpos());
  c.dfs_bidir();
  CHECK(c.at_end());
}

TEST_FIXTURE(single_cell_fixture, restart)
{
  T_cell_tracker c= g.begin();
  c.dfs();
  c.restart();
  CHECK_EQUAL(T_qpoint(0,0,0,0), c.qpos());
  CHECK_EQUAL(T_code(0,0), c.code());
}

// test that as we refine, cells shrink as they should. we can only
// check the octant 0 cell because the position of the others depend
// on the ordering
TEST_FIXTURE(single_subtree_fixture, refinement_sizes)
{
  CHECK(!c.is_leaf());
  T_qpoint rootqp=c.qpos();
  T_grid_impl* subg = c.cell()->sub_grid();
  CHECK(subg);
  c.dfs();

  CHECK_ARRAY_EQUAL(g.getmin(), c.getmin(), 3);
  CHECK_ARRAY_EQUAL(g.getmin()+0.5*g.getsize(), c.getmax(), 3);
  CHECK_ARRAY_EQUAL(0.5*g.getsize(), c.getsize(),3);
  CHECK_ARRAY_EQUAL(2/g.getsize(), c.getinvsize(),3);
  CHECK_EQUAL(1./8, c.volume());
  CHECK(rootqp.contains(c.qpos()));
  c.cell()->refine();
  c.dfs();

  CHECK_ARRAY_EQUAL(g.getmin(), c.getmin(), 3);
  CHECK_ARRAY_EQUAL(g.getmin()+0.25*g.getsize(), c.getmax(), 3);
  CHECK_ARRAY_EQUAL(0.25*g.getsize(), c.getsize(),3);
  CHECK_ARRAY_EQUAL(4/g.getsize(), c.getinvsize(),3);
  CHECK_EQUAL(1./64, c.volume());
  CHECK(rootqp.contains(c.qpos()));
}

TEST_FIXTURE(single_subtree_fixture, dfs_traversal2)
{
  CHECK_EQUAL(0, c.code().code());
  CHECK_EQUAL(0, c.code().level());
  for(int i=0; i<8; ++i) {
    c.dfs();
    CHECK_EQUAL(i, c.code().code());
    CHECK_EQUAL(1, c.code().level());
  }
  c.dfs();
  CHECK(c.at_end());
}

TEST_FIXTURE(single_subtree_fixture, bidir_dfs_traversal2)
{
  CHECK_EQUAL(0, c.code().code());
  CHECK_EQUAL(0, c.code().level());
  for(int i=0; i<8; ++i) {
    c.dfs_bidir();
    CHECK_EQUAL(i, c.code().code());
    CHECK_EQUAL(1, c.code().level());
  }
  c.dfs_bidir();
  CHECK_EQUAL(0, c.code().code());
  CHECK_EQUAL(0, c.code().level());
  c.dfs_bidir();
  CHECK(c.at_end());
}

// test that bidir traversal works correctly with higher levels
TEST_FIXTURE(single_subtree_fixture, bidir_dfs_traversal3)
{
  c=g.begin();
  CHECK_EQUAL(T_code(0,1), c.code());
  c.cell()->refine();
  c.dfs();
  for(int i=0; i<8; ++i)
    c.dfs();
  CHECK_EQUAL(T_code(1,1), c.code());
  c.cell()->refine();

  c.restart();

  CHECK_EQUAL(T_code(0,0), c.code());
  c.dfs_bidir();
  CHECK_EQUAL(T_code(0,1), c.code());
  c.dfs_bidir();

  for(int i=0; i<8; ++i) {
    CHECK_EQUAL(T_code(i,2), c.code());
    c.dfs_bidir();
  }

  CHECK_EQUAL(T_code(0,1), c.code());
  c.dfs_bidir();
  CHECK_EQUAL(T_code(1,1), c.code());
  c.dfs_bidir();

  for(int i=0; i<8; ++i) {
    CHECK_EQUAL(T_code(8+i,2), c.code());
    c.dfs_bidir();
  }

  for(int i=1; i<8; ++i) {
    CHECK_EQUAL(T_code(i,1), c.code());
    c.dfs_bidir();
  }

  CHECK_EQUAL(T_code(0,0), c.code());
  c.dfs_bidir();
  CHECK(c.at_end());
}

// Test the qpos of the subcells in the octree. Also depends on the ordering.
TEST_FIXTURE(single_subtree_fixture, octree_qpos1)
{
  CHECK_EQUAL(T_qpoint(0,0,0,0), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(0,0,0,1), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(1,0,0,1), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(1,1,0,1), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(0,1,0,1), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(0,1,1,1), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(1,1,1,1), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(1,0,1,1), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(0,0,1,1), c.qpos());
}

// Refine all cells onnce and test that the hilbert ordering flips the
// cells correctly.
TEST_FIXTURE(single_subtree_fixture, octree_qpos2)
{
  c.dfs();
  c.cell()->refine();
  c.dfs();
  CHECK_EQUAL(T_qpoint(0,0,0,2), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(0,0,1,2), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(0,1,1,2), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(0,1,0,2), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(1,1,0,2), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(1,1,1,2), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(1,0,1,2), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(1,0,0,2), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(1,0,0,1), c.qpos());
  // second octant
  c.cell()->refine();
  c.dfs();
  CHECK_EQUAL(T_qpoint(2,0,0,2), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(3,0,0,2), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(3,0,1,2), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(2,0,1,2), c.qpos());
  c.dfs();
  CHECK_EQUAL(T_qpoint(2,1,1,2), c.qpos());
  // ... and so on. if it works this far, it's hopefully ok because
  // the actual ordering is tested for all octants in the libpjutil
  // test.
}

TEST(hilbert1)
{
  hilbert_ordering h;

  for(int i=0; i<8; ++i)
    CHECK(h.octant(h.pbits(i))==i);

  for(int i=0; i<8; ++i) {
    h.push(i);
    for(int j=0; j<8; ++j)
      CHECK(h.octant(h.pbits(j))==j);
    h.pop();
  }


}


// Test that the iterator goes through the cells in cell code
// order. This test obviously fails if the grid isn't using a hilbert
// order.
TEST_FIXTURE(single_subtree_fixture, hilbert_order)
{
  using namespace hilbert;

  T_cell_tracker c= g.begin();
  cell_code code,oldcode;
  oldcode=encode_point(c.qpos());
  c.dfs();

  while(!c.at_end()) {
    code=encode_point(c.qpos());
    if(! (oldcode<code)) {
      cerr << "ordering error, " << oldcode << " !< " << code << " for cell " << c.qpos() << endl;
    }
    CHECK(oldcode<code);
    oldcode=code;
    c.dfs();
  }
}

TEST_FIXTURE(single_subtree_fixture, location)
{
  c = g.locate(vec3d(0.25,0.25,0.25), 0, false);
  CHECK(c.contains(vec3d(0.25,0.25,0.25)));

  c.locate(vec3d(0.25,0.25,0.25), false);
  CHECK(c);
  CHECK(c.contains(vec3d(0.25,0.25,0.25)));

  c.locate(vec3d(0.25,0.75,0.25), true);
  CHECK(c);
  CHECK(c.contains(vec3d(0.25,0.25,0.25)));

  c.locate(vec3d(0.25,0.75,0.25), false);
  CHECK(c.at_end());

  c = g.locate(vec3d(0.25,0.75,0.25), 0, false);
  CHECK(c.contains(vec3d(0.25,0.75,0.25)));

  c = g.locate(vec3d(0.75,0.25,0.25), 0, false);
  CHECK(c.contains(vec3d(0.75,0.25,0.25)));

  c = g.locate(vec3d(0.75,0.75,0.25), 0, false);
  CHECK(c.contains(vec3d(0.75,0.75,0.25)));

  c = g.locate(vec3d(0.75,0.75,0.25), 0, false);
  CHECK(c.contains(vec3d(0.75,0.75,0.25)));

  c = g.locate(vec3d(1.75,0.75,0.25), 0, true);
  CHECK(c.contains(vec3d(0.75,0.75,0.25)));
}

TEST_FIXTURE(single_cell_fixture, location2)
{
  T_cell_tracker c;
  c = g.locate(vec3d(0.25,0.25,0.25), 0, false);
  CHECK(c.contains(vec3d(0.25,0.25,0.25)));
}

TEST_FIXTURE(single_subtree_fixture, code_location1)
{
  T_code code(0,0);
  c.locate(code);
  CHECK_EQUAL(code, c.code());

  code=T_code(3,1);
  c.locate(code);
  CHECK_EQUAL(code, c.code());

  code=T_code(5,1);
  c.locate(code);
  CHECK(c.at_end());
}

TEST_FIXTURE(single_subtree_fixture, iterator1)
{
  T_grid::iterator i=g.begin(), e=g.end();
  int n=0;
  for(;i!=e; ++i) {
    CHECK(i.is_leaf());
    ++n;
  }
  CHECK_EQUAL(8,n);
}

TEST_FIXTURE(single_subtree_fixture, iterator1b)
{
  T_grid::iterator i=g.begin(), e=g.end();
  int n=0;
  for(;i!=e; i++) {
    CHECK(i.is_leaf());
    ++n;
  }
  CHECK_EQUAL(8,n);
}

TEST_FIXTURE(single_subtree_fixture, const_iterator1)
{
  const T_grid& gg(g);
  T_grid::const_iterator i=gg.begin(), e=gg.end();
  int n=0;
  for(;i!=e; ++i) {
    CHECK(i.is_leaf());
    ++n;
  }
  CHECK_EQUAL(8,n);
}

TEST_FIXTURE(single_subtree_fixture, iterator2)
{
  c.dfs();
  c.cell()->refine();
  c.dfs();
  c.cell()->refine();

  T_grid::iterator i=g.begin(), e=g.end();
  int n=0;
  for(;i!=e; ++i) {
    CHECK(i->is_leaf());
    CHECK((*i).is_leaf());
    ++n;
  }
  CHECK_EQUAL(22,n);
}

TEST_FIXTURE(single_subtree_fixture, iterator_copy)
{
  c.dfs();
  c.cell()->refine();
  c.dfs();
  c.cell()->refine();

  T_grid::iterator i=g.begin(), e=g.end();
  ++i;
  T_grid::iterator ii(i);
  for(;i!=e; ++i,++ii) {
    CHECK(i==ii);
    CHECK_EQUAL(i.cell(), ii.cell());
  }
  CHECK(i== ii);
}

TEST_FIXTURE(single_subtree_fixture, const_iterator2)
{
  c.dfs();
  c.cell()->refine();
  c.dfs();
  c.cell()->refine();

  const T_grid& gg(g);
  
  T_grid::const_iterator i=gg.begin(), e=gg.end();
  int n=0;
  for(;i!=e; ++i) {
    CHECK(i->is_leaf());
    CHECK((*i).is_leaf());
    ++n;
  }
  CHECK_EQUAL(22,n);
}

TEST_FIXTURE(single_subtree_fixture, n_cells1)
{
  CHECK_EQUAL(8, g.n_cells());
}

TEST_FIXTURE(single_subtree_fixture, n_cells2)
{
  c.dfs();
  c.cell()->refine();
  c.dfs();
  c.cell()->refine();
  CHECK_EQUAL(22, g.n_cells());
}

TEST_FIXTURE(single_subtree_fixture, stats)
{
  c.dfs();
  c.cell()->refine();
  c.dfs();
  c.cell()->refine();
  vector<int> stats=g.statistics();
  CHECK_EQUAL(4, stats.size());
  CHECK_EQUAL(0, stats[0]);
  CHECK_EQUAL(7, stats[1]);
  CHECK_EQUAL(7, stats[2]);
  CHECK_EQUAL(8, stats[3]);
}
   
struct raycast_fixture {
  raycast_fixture() :
    min(0,0,0), max(1,1,1), n(1,1,1), g(min, max) {

    c=g.begin();
    c.cell()->refine();

    dir=vec3d(.5,1,2);
    dir/=sqrt(dot(dir,dir));
    r.set_position(vec3d(0.25,0,0));
    r.set_direction(dir);
  }

  vec3d min;
  vec3d max;
  vec3i n;

  T_grid g;
  T_cell_tracker c;

  vec3d dir;
  ray_base r;
};

// test to find the weird alignment error in the hilbert_ordering class
TEST(raycast_fixture)
{
  vec3d min(0,0,0);
  vec3d max(1,1,1);

  asm("nop;nop;");
  T_grid g(min, max);
  asm("nop;nop;nop;");

  ray_base r;
  asm("nop;nop;nop;nop;");

  T_cell_tracker   c;
  asm("nop;nop;nop;nop;");
  //c=g.begin();
}

// Tests for casting rays in the octree.
TEST_FIXTURE(raycast_fixture, raycast1)
{
  r.set_position(vec3d(0.25,-0.25,0));
  r.set_direction(vec3d(0,1,0));
  c.raycast_dfs_push(r);
  CHECK(c.contains(vec3d(0.25,0.25,0.25)));
  CHECK(c.is_leaf());
  T_float t=c.raycast_dfs_advance(r);
  CHECK_EQUAL(0.75, t);
  CHECK(c.contains(vec3d(0.25,0.75,0.25)));
  t=c.raycast_dfs_advance(r);
  CHECK_EQUAL(1.25, t);
  CHECK(!c);
}

TEST_FIXTURE(raycast_fixture, raycast1b)
{
  r.set_position(vec3d(0.75,-0.25,0));
  r.set_direction(vec3d(0,1,0));
  c.raycast_dfs_push(r);
  CHECK(c.contains(vec3d(0.75,0.25,0.25)));
  CHECK(c.is_leaf());
  T_float t=c.raycast_dfs_advance(r);
  CHECK_EQUAL(0.75, t);
  CHECK(c.contains(vec3d(0.75,0.75,0.25)));
  t=c.raycast_dfs_advance(r);
  CHECK_EQUAL(1.25, t);
  CHECK(!c);
}


TEST_FIXTURE(raycast_fixture, raycast2)
{
  r.set_position(vec3d(0.25,1.25,0));
  r.set_direction(vec3d(0,-1,0));
  c.raycast_dfs_push(r);
  CHECK(c.contains(vec3d(0.25,0.75,0.25)));
  CHECK(c.is_leaf());
  T_float t=c.raycast_dfs_advance(r);
  CHECK_EQUAL(0.75, t);
  CHECK(c.contains(vec3d(0.25,0.25,0.25)));
  t=c.raycast_dfs_advance(r);
  CHECK_EQUAL(1.25, t);
  CHECK(!c);
}

TEST_FIXTURE(raycast_fixture, raycast3)
{
  r.set_position(vec3d(0.25,0.75,0.25));
  r.set_direction(vec3d(1,-1,0)/sqrt(2));
  c.raycast_dfs_push(r);
  CHECK(c.contains(vec3d(0.25,0.75,0.25)));
  CHECK(c.is_leaf());
  T_float t=c.raycast_dfs_advance(r);
  CHECK_CLOSE(0.25*sqrt(2), t, 1e-5);
  CHECK(c.contains(vec3d(0.75,0.25,0.25)));
  t=c.raycast_dfs_advance(r);
  CHECK_CLOSE(0.75*sqrt(2), t, 1e-5);
  CHECK(!c);
}

TEST_FIXTURE(raycast_fixture, raycast3b)
{
  r.set_position(vec3d(0.70,0.75,0.25));
  r.set_direction(vec3d(-1,-1,0)/sqrt(2));
  c.raycast_dfs_push(r);
  CHECK(c.contains(vec3d(0.75,0.75,0.25)));
  CHECK(c.is_leaf());
  T_float t=c.raycast_dfs_advance(r);
  CHECK(c.contains(vec3d(0.25,0.75,0.25)));
  t=c.raycast_dfs_advance(r);
  CHECK(c.contains(vec3d(0.25,0.25,0.25)));
  t=c.raycast_dfs_advance(r);
  CHECK(!c);
}

TEST_FIXTURE(raycast_fixture, raycast4)
{
  r.set_position(vec3d(0,0,0));
  c.raycast_dfs_push(r);
  CHECK(c.contains(vec3d(0.25,0.25,0.25)));
  CHECK(c.is_leaf());
  T_float t=c.raycast_dfs_advance(r);
  CHECK(c.contains(vec3d(0.25,0.25,0.75)));
  t=c.raycast_dfs_advance(r);
  CHECK(!c);
}

TEST_FIXTURE(raycast_fixture, raycast5)
{
  c.dfs();
  c.cell()->refine();
  c.dfs();
  c.cell()->refine();
  c.restart();
  CHECK_EQUAL(T_qpoint(0,0,0,0), c.qpos());

  r.set_position(vec3d(0,0,0));
  r.set_direction(vec3d(1,.1,0)/sqrt(1.01));

  c.raycast_dfs_push(r);
  CHECK_EQUAL(T_qpoint(0,0,0,1), c.qpos());
  c.raycast_dfs_push(r);
  CHECK_EQUAL(T_qpoint(0,0,0,2), c.qpos());
  c.raycast_dfs_push(r);
  CHECK_EQUAL(T_qpoint(0,0,0,3), c.qpos());
  CHECK(c.is_leaf());

  T_float t=c.raycast_dfs_advance(r);
  CHECK_EQUAL(T_qpoint(1,0,0,3), c.qpos());
  t=c.raycast_dfs_advance(r);
  CHECK_EQUAL(T_qpoint(1,0,0,2), c.qpos());
  t=c.raycast_dfs_advance(r);
  CHECK_EQUAL(T_qpoint(1,0,0,1), c.qpos());
  t=c.raycast_dfs_advance(r);
  CHECK(!c);
}

TEST_FIXTURE(raycast_fixture, raycast6)
{
  T_float t;
  c.dfs();
  c.cell()->refine();
  c.dfs();
  c.cell()->refine();
  c.restart();
  CHECK_EQUAL(T_qpoint(0,0,0,0), c.qpos());

  r.set_position(vec3d(.01,1,.01));
  r.set_direction(vec3d(.01,-1,.02)/sqrt(1.+5e-4));

  c.raycast_dfs_push(r);
  CHECK_EQUAL(T_qpoint(0,1,0,1), c.qpos());
  t=c.raycast_dfs_advance(r);
  CHECK_EQUAL(T_qpoint(0,0,0,1), c.qpos());
  c.raycast_dfs_push(r);
  CHECK_EQUAL(T_qpoint(0,1,0,2), c.qpos());
  t=c.raycast_dfs_advance(r);
  CHECK_EQUAL(T_qpoint(0,0,0,2), c.qpos());
  c.raycast_dfs_push(r);
  CHECK_EQUAL(T_qpoint(0,1,0,3), c.qpos());
  CHECK(c.is_leaf());

  t=c.raycast_dfs_advance(r);
  CHECK_EQUAL(T_qpoint(0,0,0,3), c.qpos());
  t=c.raycast_dfs_advance(r);
  CHECK(!c);
}

// Test the intersection_from_within function
TEST_FIXTURE(raycast_fixture, raycast7)
{
  T_float t;
  c.dfs();
  c.cell()->refine();
  c.dfs();
  c.cell()->refine();
  c.restart();
  CHECK_EQUAL(T_qpoint(0,0,0,0), c.qpos());

  r.set_position(vec3d(.01,1,.01));
  r.set_direction(vec3d(.01,-1,.02)/sqrt(1.+5e-4));

  c.raycast_dfs_push(r);
  CHECK_EQUAL(T_qpoint(0,1,0,1), c.qpos());
  t=c.raycast_dfs_advance(r);
  CHECK_EQUAL(T_qpoint(0,0,0,1), c.qpos());
  c.raycast_dfs_push(r);
  CHECK_EQUAL(T_qpoint(0,1,0,2), c.qpos());
  t=c.raycast_dfs_advance(r);
  CHECK_EQUAL(T_qpoint(0,0,0,2), c.qpos());
  c.raycast_dfs_push(r);
  CHECK_EQUAL(T_qpoint(0,1,0,3), c.qpos());
  CHECK(c.is_leaf());

  t=c.raycast_dfs_advance(r);
  CHECK_EQUAL(T_qpoint(0,0,0,3), c.qpos());
  t=c.raycast_dfs_advance(r);
  CHECK(!c);
}
